﻿using NCM_MSTest.Utils;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Reflection.Emit;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Resolvers;

namespace NCM_MSTest.Alg
{
    /// <summary>
    /// 线性代数方程组的求解器
    /// <br></br>
    /// 直接法：
    ///     将线性方程组转换为便于求解的三角线性方程组
    ///     ，再求三角线性方程组的数值解（由于数值运算存在舍入因此也是近似解）
    /// <br></br>
    /// 迭代法：
    ///     将求解线性方程组的问题转化为构造一个无穷序列
    ///     ，并用这个无穷序列去逐次逼近满足一定误差（精度）要求的方程组的数值解
    ///<br></br>
    ///优缺点：
    ///<br></br>
    /// 直接法：
    ///     所需内存随线性方程组方程的个数增加而增大
    ///     适用于低阶线性方程组，计算量小，计算精度高，但计算流程复杂
    ///     <br></br>
    /// 迭代法：
    ///     适用于高阶线性方程组的求解，方法和计算流程简单
    ///     ，但要得到精度较高的数值解，需要较大的计算量
    /// </summary>
    public class NumbericalSolutionOfLinearEquations
    {
        /// <summary>
        /// 直接解法-高斯消去法
        /// 将线性方程组通过n步逐次消元，使其转化为系数矩阵主对角线元素为1的右上三角线性方程组
        /// 最后一个方程组只含一个未知数
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        public static void Direct_Gauss(double[,] A, double[] B, ref double[] X)
        {
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            int m = n + 1;
            double[,] eA = new double[n, m]; //增广矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < m; j++)
                {
                    if (j == m - 1)
                        eA[i, j] = B[i];
                    else
                        eA[i, j] = A[i, j];
                }
            }
            Console.WriteLine("增广矩阵:");
            eA.Print();
            for (int i = 0; i < n; i++)
            {
                //使主对角线上元素不为0
                int jj = i;
                //若对角线元素为0，则查找同列大于i行的不为0的行
                while (eA[jj, i] == 0 && jj < n)
                {
                    jj++;
                }
                if (i != jj)
                {
                    //交换 行i <===> 行jj
                    //注：i列之前的列可以不用交换，因为不参与后面的运算
                    for (int mm = 0; mm < m; mm++)
                    {
                        double t = eA[i, mm];
                        eA[i, mm] = eA[jj, mm];
                        eA[jj, mm] = t;
                    }
                }
                //主对角线元素化为1，其同列下面行的元素化为0
                double div = eA[i, i];
                for (int j = i; j < m; j++)
                {
                    eA[i, j] = eA[i, j] / div;
                }
                int k = i + 1;
                if (k <= m - 1)
                {
                    for (int L = k; L < n; L++)
                    {
                        double amult = eA[L, i];
                        for (int j = i; j < m; j++)
                        {
                            eA[L, j] = eA[L, j] - eA[i, j] * amult;
                        }
                    }
                }
                //Matrix.Print(eA);
            }
            Console.WriteLine("右上三角矩阵:");
            eA.Print();
            X[n - 1] = eA[n - 1, m - 1];
            //回代过程
            int k1 = n - 1;
            for (int L = 0; L < n - 1; L++)
            {
                k1 = k1 - 1;
                double sum = 0;
                for (int j = k1 + 1; j < n; j++)
                {
                    sum = sum + eA[k1, j] * X[j];
                }
                X[k1] = eA[k1, m - 1] - sum;
            }
        }

        /// <summary>
        /// 直接解法-高斯-约当消去法
        /// 对增广矩阵进行变换：系数矩阵的主对角线元素为1，非主对角线元素为0，常数项值即为方程组的解
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        /// <exception cref="ArgumentException"></exception>
        public static void Direct_GaussJordan(double[,] A, double[] B, ref double[] X)
        {
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            int m = n + 1;
            double[,] eA = new double[n, m]; //增广矩阵
            for (int i = 0; i < n; i++)
            {
                for (int j = 0; j < m; j++)
                {
                    if (j == m - 1)
                        eA[i, j] = B[i];
                    else
                        eA[i, j] = A[i, j];
                }
            }
            Console.WriteLine("增广矩阵:");
            eA.Print();
            for (int i = 0; i < n; i++)
            {
                //使主对角线上元素不为0
                int jj = i;
                //若对角线元素为0，则查找同列大于i行的不为0的行
                while (eA[jj, i] == 0 && jj < n)
                {
                    jj++;
                }
                if (i != jj)
                {
                    //交换 行i <===> 行jj
                    //注：i列之前的列可以不用交换，因为不参与后面的运算
                    for (int mm = 0; mm < m; mm++)
                    {
                        double t = eA[i, mm];
                        eA[i, mm] = eA[jj, mm];
                        eA[jj, mm] = t;
                    }
                }
                //主对角线元素化为1，其同列其他的元素化为0
                double div = eA[i, i];
                for (int j = i; j < m; j++)
                {
                    eA[i, j] = eA[i, j] / div;
                }
                for (int L = 0; L < n; L++)
                {
                    if (L != i)
                    {
                        double amult = eA[L, i];
                        for (int j = 0; j < m; j++)
                        //for (int j = i + 1; j < m; j++) 
                        //此处优化不影响后续计算，左下三角矩阵不参与后续计算
                        {
                            eA[L, j] = eA[L, j] - eA[i, j] * amult;
                        }
                    }
                }
                Console.WriteLine("----");
                eA.Print();
            }
            Console.WriteLine("系数矩阵为单位矩阵:");
            eA.Print();
            for (int i = 0; i < n; i++)
            {
                X[i] = eA[i, m - 1];
            }
        }

        /// <summary>
        /// 直接解法-追赶法
        /// 适用矩阵：系数矩阵三对角线元素有值，其他位置为0的线性方程组
        /// 三对角线带状矩阵
        /// 求解：只对系数矩阵的非0元素赋值
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        /// <exception cref="ArgumentException"></exception>
        public static void Direct_Trid(double[,] A, double[] B, ref double[] X)
        {
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            //检测是否为：三对角线带状矩阵
            for (int i = 0; i < A.GetLength(0); i++)
            {
                for (int j = 0; j < A.GetLength(1); j++)
                {
                    if (Math.Abs(j - i) > 1)
                        if (A[i, j] != 0)
                            throw new ArgumentException(string.Format("A[{0},{1}]", i, j));
                }
            }
            int n = A.GetLength(0);
            double[] a = new double[n - 1];
            double[] b = B;
            double[] c = new double[n - 1];
            double[] d = new double[n];
            //初始化对角元素
            for (int i = 0; i < n; i++)
            {
                if (i != 0)
                    a[i - 1] = A[i - 1, i];
                if (i != n - 1)
                    c[i] = A[i + 1, i];
                d[i] = A[i, i];
            }

            double[] a1 = new double[n];
            double[] b1 = new double[n];
            //Alpha_1 = c_1 / d_1   Beta_1 = b_1 / d_1
            //Alpha_i = c_i / (d_i - Alpha_{i-1} * a_{i-1})
            //Beta_i = (b_i - Beta_{i-1} *a_{i-1}) / (d_i - Alpha_{i-1} * a_{i-1})
            for (int i = 1; i < n; i++)
            {
                int m = i - 1;
                a1[m] = c[m] / d[m];
                b1[m] = b[m] / d[m];
                d[i] = d[i] - a1[m] * a[m];
                b[i] = b[i] - b1[m] * a[m];
            }
            //x_n = Beta_n
            b[n - 1] = b[n - 1] / d[n - 1];
            //x_{n-1} = Beta_i - Alpha_i * x_{i+1}
            for (int i = 0; i < n - 1; i++)
            {
                int k = n - 2 - i;
                b[k] = b1[k] - a1[k] * b[k + 1];
            }
            for (int i = 0; i < b.Length; i++)
            {
                X[i] = b[i];
            }
        }

        /// <summary>
        /// 迭代解法 - 雅可比迭代法
        /// 使用矩阵：系数矩阵中的主对角线上的元素均不为0
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        /// <param name="k"></param>
        /// <param name="eps"></param>
        /// <param name="L"></param>
        /// <exception cref="ArgumentException"></exception>
        public static void Iteration_Jacobi(double[,] A, double[] B, ref double[] X
            , out int k //运算过程中的迭代次数
            , double eps = 1e-4 //给定的误差要求
            , int L = 100 //最大迭代次数
            )
        {
            k = 0;
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            for (int i = 0; i < n; i++)
            {
                if (A[i, i] == 0)
                    throw new ArgumentException(string.Format("A[{0},{1}]", i, i));
            }
            //根据输入构造M矩阵、f矩阵
            //double[,] M = new double[n, n]; //M=A
            //double[] f = new double[n]; //f=B
            for (int i = 0; i < n; i++)
            {
                double delt = A[i, i];
                B[i] = B[i] / delt;
                for (int j = 0; j < n; j++)
                {
                    A[i, j] = A[i, j] / delt;
                }
                A[i, i] = 0;
                X[i] = 0;
            }
            double[] Y = new double[n];
            do
            {
                if (k > L)
                    break;//输出X
                k++;
                int m = 0;
                //第k次迭代得到Y
                for (int i = 0; i < n; i++)
                {
                    double sum = 0;
                    for (int j = 0; j < n; j++)
                    {
                        sum = sum + A[i, j] * X[j];
                    }
                    Y[i] = B[i] - sum;
                }
                for (int j = 0; j < n; j++)
                {
                    if (Math.Abs(Y[j] - X[j]) < eps)
                        m++;
                }
                Array.Copy(Y, X, Y.Length);
                if (m >= n)
                    break;//输出X
                Console.WriteLine(string.Format("k={0}  x= {1}", k, X.ToStringEx()));
            } while (true);
        }

        /// <summary>
        /// 迭代解法 - 雅可比迭代法（优化）
        /// 使用矩阵：系数矩阵中的主对角线上的元素均不为0
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        /// <param name="k"></param>
        /// <param name="eps"></param>
        /// <param name="L"></param>
        /// <exception cref="ArgumentException"></exception>
        public static void Iteration_Jacobi2(double[,] A, double[] B, ref double[] X
            , out int k //运算过程中的迭代次数
            , double eps = 1e-4 //给定的误差要求
            , int L = 100 //最大迭代次数
            )
        {
            k = 0;
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            for (int i = 0; i < n; i++)
            {
                if (A[i, i] == 0)
                    throw new ArgumentException(string.Format("A[{0},{1}]", i, i));
            }
            for (k = 0; k < L; k++)
            {
                double[] Y = new double[n];
                int m = 1;
                //迭代求解,得到Y,m
                for (int i = 0; i < n; i++)
                {
                    double sum = 0;
                    for (int j = 0; j < n; j++)
                    {
                        if (i != j)
                            sum = sum + A[i, j] * X[j];
                    }
                    Y[i] = (B[i] - sum) / A[i, i];
                    if (Math.Abs(Y[i] - X[i]) > eps)
                        m = 0;
                }
                Array.Copy(Y, X, Y.Length);
                if (m != 0)
                    break;
                Console.WriteLine(string.Format("k={0}  x= {1}", k, X.ToStringEx()));
            }
        }

        public static void Iteration_Jacobi_Matrix(double[,] A, double[] B, ref double[] X
            , out int k //运算过程中的迭代次数
            , double eps = 1e-4 //给定的误差要求
            , int L = 100 //最大迭代次数
            )
        {
            k = 0;
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            for (int i = 0; i < n; i++)
            {
                if (A[i, i] == 0)
                    throw new ArgumentException(string.Format("A[{0},{1}]", i, i));
            }
            /*
            提取矩阵D，U，L
            M= D/(L+U); 
            F = D /b;
            Y = M * X + F; 
            K=1;
            while(norm(Y-X)>eps && k<=n)
            {
                X=Y;
                Y=M*X+f;
                k=k+1;
            }
            //输出结果Y
             */
        }

        /// <summary>
        /// 迭代解法 - 高斯赛德尔迭代法（优化）
        /// 使用矩阵：系数矩阵中的主对角线上的元素均不为0
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="X"></param>
        /// <param name="k"></param>
        /// <param name="eps"></param>
        /// <param name="L"></param>
        /// <exception cref="ArgumentException"></exception>
        public static void Iteration_GaussSeidel(double[,] A, double[] B, ref double[] X
            , out int k //运算过程中的迭代次数
            , double eps = 1e-4 //给定的误差要求
            , int L = 100 //最大迭代次数
            )
        {
            k = 0;
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            for (int i = 0; i < n; i++)
            {
                if (A[i, i] == 0)
                    throw new ArgumentException(string.Format("A[{0},{1}]", i, i));
                X[i] = 0;
            }
            for (k = 0; k < L; k++)
            {
                int m = 1;
                //迭代求解，得到X和m
                for (int i = 0; i < n; i++)
                {
                    double delt = X[i];
                    double sum = 0;
                    for (int j = 0; j < n; j++)
                    {
                        if (j != i)
                        {
                            sum = sum + A[i, j] * X[j];
                        }
                    }
                    X[i] = (B[i] - sum) / A[i, i];
                    if (Math.Abs(delt - X[i]) > eps)
                        m = 0;
                }
                if (m != 0)
                    break;
                Console.WriteLine(string.Format("k={0}  x= {1}", k, X.ToStringEx()));
            }
        }


        public static void Iteration_GaussSeidel_Matrix(double[,] A, double[] B, ref double[] X
            , out int k //运算过程中的迭代次数
            , double eps = 1e-5 //给定的误差要求
            , int L = 100 //最大迭代次数
            )
        {
            k = 0;
            if (A.GetLength(0) != A.GetLength(1)
                || A.GetLength(0) != B.GetLength(0)
                || A.GetLength(0) != X.GetLength(0))
                throw new ArgumentException();
            int n = A.GetLength(0);
            for (int i = 0; i < n; i++)
            {
                if (A[i, i] == 0)
                    throw new ArgumentException(string.Format("A[{0},{1}]", i, i));
                X[i] = 0;
            }
            /*
            提取矩阵D，U，L
            M= (D-L)/U; 
            f = (D-L)/b;
            Y = M * X + F; 
            k=1;
            while(norm(Y-X)>eps && k<=n)
            {
                X=Y;
                Y=M*X+f;
                k=k+1;
            }
            //输出结果Y
             */
        }
    }
}
